#include "Mymath.h"

FP32 MyFP32Min(FP32 x, FP32 y) {
	return x < y ? x : y;
}

FP32 MyFP32Max(FP32 x, FP32 y) {
	return x > y ? x : y;
}

FP32 MyFP32Abs(FP32 x)
{
    return x > 0 ? x : (-x);
}

INT16U MyINT16UAbs(INT16U x, INT16U y)
{
    return x > y ? (x - y) : (y - x);
}

/*
*函数名：convolution
*功  能：求两个输入卷积
*输入参数：
    FP64 *Input1        输入1数组
    FP64 *Input2        输入2数组
    INT16U Number1      输入1点数
    INT16U Number2      输入2点数
    FP64* Output        输出数组（长度为NUM1+NUM2-1）
*返回参数：None
*/
void Convolution(FP32* input1, FP32* input2, FP32* output, INT16U mm, INT16U nn)
{
	//存储地址
	FP32* xx = (FP32*)malloc(sizeof(FP32) * (mm + nn - 1));
	//开始卷积
	for (INT16U i = 0; i < mm + nn - 1; i++)
	{
		xx[i] = 0;
		//以位数最少的卷积作为卷积次数
		for (INT16U j = 0; j < MyFP32Min(mm,nn) ; j++)
        {
            //第一个卷积比第二个卷数积少执行
            if (mm <= nn)
            {
				if (i - j >= 0 && i - j < MyFP32Max(mm, nn))
				{
					xx[i] += input1[j] * input2[i - j];
				}
			}
			//第一个卷积比第二个卷积数多执行
			else
			{
				if (i - j >= 0 && i - j < MyFP32Max(mm, nn))
				{
					xx[i] += input2[j] * input1[i - j];
				}
			}
		}
	}
	for (INT16U i = 0; i < mm+nn-1; i++)
    {
		output[i] = xx[i];
	}
	free(xx);
}


/*
取得某一位的数字，十进制
*/
INT16U GetDigit(INT32U Number, INT8U Digit)
{
    INT16U Result;
    
    Result = ((INT32U)(Number / Digit)) % 10;
    
    return Result;
}

/*
设置某一位的数字，十进制
*/
void SetDigit(INT32U *Number, INT8U Digit, INT8U Value)
{
    INT16U Result;
    
    Result = ((INT32U)(*Number / Digit)) % 10;
    
    if(Number == NULL)
    {
        return;
    }
    *Number -= (Result * Digit);
    
    *Number += (Value * Digit);
}

void BubbleSortINT16(INT16U *p , INT16U Length)
{
    INT16U ii = 0 , jj = 0;
    INT16U Temp = 0;

    for(ii = 0 ; ii < Length - 1 ; ii++)
    {
        for (jj = 0 ; jj < Length - 1 - ii ; jj++)
        {
            if (p[jj] > p [jj + 1])
            {
                Temp = p[jj];
                p[jj] = p[jj + 1];
                p[jj + 1] = Temp;
            }
        }
    }
}

void BubbleSortFP32(FP32 *p , INT16U Length)
{
    INT16U ii = 0 , jj = 0;
    FP32 Temp = 0;

    for(ii = 0 ; ii < Length - 1 ; ii++)
    {
        for (jj = 0 ; jj < Length - 1 - ii ; jj++)
        {
            if (p[jj] > p[jj + 1])
            {
                Temp = p[jj];
                p[jj] = p[jj + 1];
                p[jj + 1] = Temp;
            }
        }
    }    
}


INT32U MyPow(INT32U X, INT32U Y)
{
    INT32U Result;
    INT16U ii;
    
    Result = X;
    if(Y == 0)
    {
        return 1;
    }
    
    for(ii = 0 ; ii < Y - 1 ; ii++)
    {
        Result *= X;
    }
    
    return Result;
}

/**
* @brief 四舍五入保留小数
* @param Keep   要保留的位数
* @param Data   输入数据
* @return 保留后的小数
*/
FP32 KeepDecimals(INT8U Keep, FP32 Data)
{
    INT32U Shift;
    FP32 Result;
	INT8U Neg = 0;
    
	if (Data < 0)
	{
		Neg = 1;
	}
    
    Shift = MyPow(10,(Keep + 1));
    
	if (Neg)
	{
        Data -= ((1.0f / (FP32)Shift) * 5.0f);
    }
    else
    {
        Data += ((1.0f / (FP32)Shift) * 5.0f);
    }
    Data *= (Shift/10);
    
    Result = (INT32U)Data;
    
	if (Neg)
	{
		return -(Result/(Shift/10));
	}
	else
	{
		return Result/(Shift/10);
	}
}

/**
* @brief 最小二乘线性拟合
* @param X      横坐标
* @param Y      纵坐标
* @param Number 坐标点数量
* @param ParaK  拟合直线斜率（指针）
* @param ParaB  拟合直线截距（指针）
* @return none
*/
void LeastSquareLinearFit(float X[], float Y[], const int Number, float *ParaK, float *ParaB)
{
    float SumXX = 0.0f;
    float SumY = 0.0f;
    float SumX = 0.0f;
    float SumXY = 0.0f;
    
	if(Number <= 1)
	{
		*ParaK = 0;
		*ParaB = 0;
		return;
	}

    for(int ii = 0 ; ii < Number ; ii++)
    {
        SumXX += X[ii] * X[ii];
        SumY += Y[ii];
        SumX += X[ii];
        SumXY += X[ii] * Y[ii];
    }

    *ParaK = (Number * SumXY - SumX * SumY) / (Number * SumXX - SumX * SumX);
    *ParaB = (SumXX * SumY - SumX * SumXY) / (Number * SumXX - SumX * SumX);
}
